#include "klt.hpp"

KLT::KLT(Process *pro):Observer(pro)
{
	nFeatures = 500;
	nFrames = 1000;
	ccount = 1;
	
	tc = KLTCreateTrackingContext();
	tc->mindist = 20;
	tc->window_width  = 9;
	tc->window_height = 9;
	tc->sequentialMode = TRUE;
	tc->writeInternalImages = false;
	tc->affineConsistencyCheck = -1;  /* set this to 2 to turn on affine consistency check */	
	KLTChangeTCPyramid(tc, 15);
	KLTUpdateTCBorder(tc);


	fl = KLTCreateFeatureList(nFeatures);
	ft = KLTCreateFeatureTable(nFrames, nFeatures);

}

KLT::~KLT()
{
	Log("Release KLT");
	KLTWriteFeatureTable(ft, "features.txt", "%5.1f");
	
	_pro->Detach(this);
}
	 
void KLT::Update(const Image &image)
{
	Image gray, prev_gray;
	
	if(_pro->m_count == 1)
	{
	}
	else if(_pro->m_count > 1)
	{
		image.Convert(gray);
		
		KLTSelectGoodFeatures(tc, gray.data, gray.width, gray.height, fl);
		KLTStoreFeatureList(fl, ft, 0);
	}
	else
	{
		image.Convert(gray);
		_pro->prv_image.Convert(prev_gray);
		KLTTrackFeatures(tc, prev_gray.data, gray.data, gray.width, gray.height, fl);
		
		//*/ ReplaceLostFeatures
		KLTReplaceLostFeatures(tc, gray.data, gray.width, gray.height, fl);

		//KLTWriteFeatureListToPPM(fl, gray.data, gray.width, gray.height, fnameout);
		//*/
		KLTStoreFeatureList(fl, ft, ccount++);
	}
}

void KLT::Draw(Image &image)
{
	for (int i = 0 ; i < fl->nFeatures ; i++)
		if (fl->feature[i]->val >= 0)
		{
			int x = (int) (fl->feature[i]->x + 0.5);
			int y = (int) (fl->feature[i]->y + 0.5);
			for (int yy = y - 1 ; yy <= y + 1 ; yy++)
			for (int xx = x - 1 ; xx <= x + 1 ; xx++)  
				if (xx >= 0 && yy >= 0 && xx < image.width && yy < image.height)
				{
					image[yy][xx*3] = 255;
					image[yy][xx*3 + 1] = 0;
					image[yy][xx*3 + 2] = 0;
				}
		}
}
